from distutils.log import info
import math
from re import T
from turtle import Turtle
from Source.CWJ.qyhf import check
from Method import collision_detection, get_Intersection_points, get_cos_value, blocks_select
import copy
from Avoidance import Avoid_left_right, Avoid_up_down, pipe_preprocess, Avoid_vertical, Avoid_move, Avoid_wall
import numpy as np


class Node(object):
    def __init__(self, label, arr):
        self.label = label
        self.prev = None
        self.next = None
        self.arr = arr


class LinkedList(object):
    def __init__(self):
        self.head = None
        self.processed_pipes = []
        self.label_count = 0

    def is_Empty(self):  # 判断链表是否为空
        return self.head is None

    def create_LinkedList(self, pilist_arr):  # 输入管道列信息，创建双向链表
        for i in range(len(pilist_arr)):
            self.append_Node(pilist_arr[i])

    def add_Node(self, pipe_arr):  # 链表头部添加结点
        node = Node(self.label_count, pipe_arr)
        self.label_count += 1
        if self.is_Empty():
            self.head = node
        else:
            cursor = self.head
            cursor.prev = node
            node.next = cursor
            self.head = node

    def append_Node(self, pipe_arr):  # 链表尾部添加结点
        node = Node(self.label_count, pipe_arr)
        self.label_count += 1
        if self.is_Empty():
            self.head = node
        else:
            cursor = self.head
            while cursor.next is not None:
                cursor = cursor.next
            cursor.next = node
            node.prev = cursor

    def insert_Node(self, label, pipe_arr):  # 在label位置处插入结点
        if self.is_Empty():
            print('链表为空，无法插入结点！')
        else:
            cursor = self.head
            while cursor is not None:
                if cursor.label == label:
                    if cursor == self.head:
                        self.add_Node(pipe_arr)
                    else:
                        node = Node(self.label_count, pipe_arr)
                        self.label_count += 1
                        node.next = cursor
                        node.prev = cursor.prev
                        cursor.prev.next = node
                        cursor.prev = node
                    break
                else:
                    cursor = cursor.next

    def delete_Node(self, label):  # 删除label位置处的结点
        if self.is_Empty():
            print('链表为空，无法删除结点！')
        else:
            cursor = self.head
            while cursor is not None:
                if cursor.label == label:
                    if cursor == self.head:
                        self.head = cursor.next
                        if cursor.next is not None:
                            cursor.next.prev = None
                    else:
                        cursor.prev.next = cursor.next
                        if cursor.next is not None:
                            cursor.next.prev = cursor.prev
                    break
                else:
                    cursor = cursor.next

    def output_Arr(self):
        array = []
        cursor = self.head
        while cursor is not None:
            array.append(cursor.arr)
            cursor = cursor.next
        return array


def Avoid_obstacles(last_pi, pi, stru, block_list, temp, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
    # pi[5] = "进行了避障"
    # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置]
    if pi[0][2] > ceilling[1] or pi[0][2] < ceilling[0]:
        information = [[[pi], [0, 0, 0]], 'Fail', None, None, [None, pi[1]], None]

    if temp == 'Pipe':  # 若碰撞物是水管或风管
        print('障碍物为管道')
        information = Avoid_PipeOrDuct(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling,
                                       endpipe_flag)

    elif temp == 'Frame':  # 若碰撞物是横梁，上下避障
        print('障碍物为横梁')
        information = Avoid_Frame(last_pi, pi, stru, block_list, BlockBase, wall_list, "Change to down", ceilling,
                                  endpipe_flag)

    elif temp == 'Column':  # 若碰撞物是柱子，左右避障
        print('障碍物为柱子')
        information = Avoid_Column(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling,
                                   endpipe_flag)

    else:
        information = [[[pi], [0, 0, 0]], 'Fail', None, None, [None, None], None]

    if information[1] == 'Done':
        information[0][0][0][5] = "避障成功"
        print('当前管道段避障成功', information)
        return information[0][0], information[0][1], information[3], information[4], information[5]

    else:
        pi[5] = "避障失败"
        print('当前管道段避障失败！返回原管道：', pi)
        # plot_wall([pi])
        return [pi], [0, 0, 0], None, information[4], information[5]


def Avoid_Column(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
    # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
    information = [[[pi], [0, 0, 0]], 'Enter general avoidance', None, None, [None, None], None]

    if information[0][0][0][0][0] == information[0][0][0][1][0] and information[0][0][0][0][1] == \
            information[0][0][0][1][1]:  # 若管道为竖直
        return [[[pi], [0, 0, 0]], 'Done', None, None, [None, None], None]

    cos_value = get_cos_value(pi, stru)
    intersection_points = get_Intersection_points(information[0][0][0], stru)
    if 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
        information = pipe_preprocess(pi, stru, cos_value, PipeGroup_Type)
        if collision_detection(information[0][0][0], stru):
            information[1] = 'Enter translational avoidance'
        else:
            return information
    elif cos_value == 1 or cos_value == -1:
        information[1] = 'Enter translational avoidance'
    elif intersection_points == []:
        information[1] = 'Enter translational avoidance'

    if information[1] == 'Enter general avoidance':
        information = Avoid_left_right(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type,
                                       information[0][1], ceilling, Avoid_Column_flag=True)
    if information[1] == 'Enter translational avoidance':
        information = Avoid_move(last_pi, information[0][0][0], stru, block_list, PipeGroup_Type, BlockBase, wall_list,
                                 information[0][1], ceilling, endpipe_flag, information[2], information[4])
    return information


def Avoid_Frame(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
    # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
    information = [[[pi], [0, 0, 0]], 'Enter general avoidance', None, None, [None, None], None]

    if information[0][0][0][0][0] == information[0][0][0][1][0] and information[0][0][0][0][1] == \
            information[0][0][0][1][1]:  # 若管道为竖直
        return [[[pi], [0, 0, 0]], 'Done', None, None, [None, None], None]

    # cos_value = get_cos_value(pi, stru)
    # intersection_points = get_Intersection_points(information[0][0][0], stru)  # 获取预处理后的管道和障碍物的碰撞坐标
    # if 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
    #     information = pipe_preprocess(pi, stru, cos_value, PipeGroup_Type)
    #     if collision_detection(information[0][0][0], stru):
    #         information[1] = 'Enter translational avoidance'
    #     else:
    #         return information
    # elif cos_value == 1 or cos_value == -1:
    #     information[1] = 'Enter translational avoidance'
    # elif intersection_points == []:
    #     information[1] = 'Enter translational avoidance'

    if information[1] == 'Enter general avoidance':
        information = Avoid_up_down(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type,
                                    information[0][1], ceilling)

    if information[1] == 'Enter translational avoidance':
        information = Avoid_move(last_pi, information[0][0][0], stru, block_list, PipeGroup_Type, BlockBase, wall_list,
                                 information[0][1], ceilling, endpipe_flag, information[2], information[4])
    return information


def Avoid_PipeOrDuct(last_pi, pi, stru, block_list, BlockBase, wall_list, PipeGroup_Type, ceilling, endpipe_flag):
    # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
    information = [[[pi], [0, 0, 0]], 'Enter general avoidance', None, None, [None, None], None]

    if information[0][0][0][0][0] == information[0][0][0][1][0] and information[0][0][0][0][1] == \
            information[0][0][0][1][1]:  # 若管道为竖直
        return [[[pi], [0, 0, 0]], 'Done', None, None, [None, None], None]

    cos_value = get_cos_value(pi, stru)
    intersection_points = get_Intersection_points(information[0][0][0], stru)  # 获取预处理后的管道和障碍物的碰撞坐标
    if intersection_points == []:
        information[1] = 'Enter translational avoidance'
    elif 0.966 < cos_value < 1 or -1 < cos_value < -0.966:
        information = pipe_preprocess(pi, stru, cos_value, PipeGroup_Type)
        if collision_detection(information[0][0][0], stru):
            information[1] = 'Enter general avoidance'
        else:
            return information
    elif cos_value == 1 or cos_value == -1:
        information[1] = 'Enter general avoidance'

    if information[1] == 'Enter general avoidance':
        information = Avoid_left_right(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type,
                                       information[0][1], ceilling, Avoid_Column_flag=False)
        if information[1] == 'Change to up-down':
            information = Avoid_up_down(information[0][0][0], stru, block_list, BlockBase, wall_list, PipeGroup_Type,
                                        information[0][1], ceilling)
    if information[1] == 'Enter translational avoidance':
        information = Avoid_move(last_pi, information[0][0][0], stru, block_list, PipeGroup_Type, BlockBase, wall_list,
                                 information[0][1], ceilling, endpipe_flag, information[2], information[4])
    return information


def pilist_avoidance(linkedlist, block_list, blockBase, PipeGroup_Type, ceilling, pipeBase):  # 若同一列发生碰撞，则上下避开
    change_flag = False
    unchangedlen_linkedlist = copy.deepcopy(linkedlist)
    cursor = linkedlist.head  # 将当前结点设为头结点

    while cursor is not None:
        other_linkedlist = copy.deepcopy(linkedlist)  # 除了被避障管道及其相邻管道的其他管道
        other_add_linkedlist = copy.deepcopy(linkedlist)  # 除了被避障管道、避障管道及其前一段管道的其他管道
        other_linkedlist.delete_Node(cursor.label)  # 删除当前结点的前一个结点
        if cursor.next is not None:
            other_linkedlist.delete_Node(cursor.next.label)  # 删除当前结点的后一个结点
        if cursor.prev is not None:
            other_linkedlist.delete_Node(cursor.prev.label)  # 删除当前结点的前一个节点
        other_add_linkedlist.delete_Node(cursor.label)  # 删除当前结点

        cursor2 = other_linkedlist.head  # 将当前结点设为头结点
        collision_node = None
        Currentpipe_fail_avoid = False  # 当前管道段避障失败的标志
        while cursor2 is not None:
            if [cursor.label, cursor2.label] in linkedlist.processed_pipes:
                cursor2 = cursor2.next
                continue
            is_collision = collision_detection(cursor.arr, cursor2.arr)
            if is_collision:
                collision_node = cursor2
                other_add_linkedlist.delete_Node(cursor2.label)  # 删除当前结点
                if cursor2.prev is not None:
                    other_add_linkedlist.delete_Node(cursor2.prev.label)  # 删除当前结点的前一个结点
                break
            cursor2 = cursor2.next

        if collision_node == None:
            cursor = cursor.next
            continue
        else:
            linkedlist.processed_pipes.append([cursor.label, collision_node.label])
            linkedlist.processed_pipes.append([collision_node.label, cursor.label])
            pipe = collision_node.arr
            block = cursor.arr
            for i in range(len(block_list)):  # 添加障碍物，用于输入避障函数
                other_add_linkedlist.append_Node(block_list[i])

            other_add = other_add_linkedlist.output_Arr()  # 获取数组形式的管道

            # information信息:[[新管道， 修正值], 状态, [上下避障的新管道, 上下避障后的修正值], 新的上一段管道, 超过天花板所处位置] #
            information = Avoid_up_down(pipe, block, other_add, blockBase, [], PipeGroup_Type, [0, 0, 0], ceilling)
            if information[4][0] is None:
                new_pi = information[0][0]
                rectify = information[0][1]
            else:
                new_pi = information[2][0]
                rectify = information[2][1]
                cross_position = information[4][0]
                pipeBase.yellowBallCoordinates.append(cross_position)

            if new_pi != [pipe]:  # 若生成新的管道，将后面的管道衔接上
                change_flag = True  # 避障标志，True表示该管道已避障
                cursor3 = unchangedlen_linkedlist.head
                while cursor3 is not None:
                    if cursor3.label == collision_node.label:
                        break
                    else:
                        cursor3 = cursor3.next
                cursor3 = cursor3.next  # 从当前管道的下一段管道开始
                while cursor3 is not None:
                    cursor3.arr[0] = [cursor3.arr[0][i] + rectify[i] for i in range(3)]
                    cursor3.arr[1] = [cursor3.arr[1][i] + rectify[i] for i in range(3)]
                    cursor3 = cursor3.next
            else:
                Currentpipe_fail_avoid = True

            if not Currentpipe_fail_avoid:  # 若当前管道段避障成功或标球，则更新:
                linkedlist.delete_Node(collision_node.label)
                if collision_node.next is not None:
                    insert_location = collision_node.next.label
                else:
                    insert_location = None
                for i in range(len(new_pi)):
                    if insert_location is not None:
                        linkedlist.insert_Node(insert_location, new_pi[i])
                    else:
                        linkedlist.append_Node(new_pi[i])

                cursor5 = linkedlist.head
                while cursor5 is not None:
                    if cursor5.label == insert_location:
                        break
                    else:
                        cursor5 = cursor5.next
                while cursor5 is not None:
                    cursor5.arr[0] = [cursor5.arr[0][i] + rectify[i] for i in range(3)]
                    cursor5.arr[1] = [cursor5.arr[1][i] + rectify[i] for i in range(3)]
                    cursor5 = cursor5.next
            else:
                pass
            break

    if cursor is not None:
        change_flag_recursion = pilist_avoidance(linkedlist, block_list, blockBase, PipeGroup_Type, ceilling, pipeBase)
        change_flag = change_flag or change_flag_recursion
        return change_flag
    else:  # 若遍历完管道列的最后一段，则返回
        return change_flag


def pilist_update(pilist, unchanged_len_pilist, j, information):
    find_id = pilist.index(unchanged_len_pilist[j])
    new_pi = information[0]
    rectify = information[1]
    new_last_pi = information[2]
    for o in range(j + 1, len(unchanged_len_pilist)):
        unchanged_len_pilist[o][0] = [unchanged_len_pilist[o][0][i] + rectify[i] for i in range(3)]
        unchanged_len_pilist[o][1] = [unchanged_len_pilist[o][1][i] + rectify[i] for i in range(3)]
    if new_last_pi is not None:
        unchanged_len_pilist[j - 1] = new_last_pi
    unchanged_len_pilist[j] = new_pi
    # todo
    pilist.pop(find_id)
    pilist.insert(find_id, new_pi)
    for o in range(find_id + 1, len(pilist)):
        pilist[o][0] = [pilist[o][0][i] + rectify[i] for i in range(3)]
        pilist[o][1] = [pilist[o][1][i] + rectify[i] for i in range(3)]
    if new_last_pi is not None:  # 若发生平移避障，则需更新前一段管道
        # 如果管道中有管道方向与平移移动方向平行的
        flag = False
        for index in range(find_id - 1, -1, -1):
            pipe_vector = [pilist[index][1][i] - pilist[index][0][i] for i in range(3)]
            pipe_length = math.sqrt(sum(e ** 2 for e in pipe_vector))
            block_length = math.sqrt(sum(e ** 2 for e in rectify))
            cos_value = round(np.dot(pipe_vector, rectify) / pipe_length / block_length, 4)
            print(cos_value)
            if -1 <= cos_value <= -0.9659 or 0.9659 <= cos_value <= 1 and pipe_length > block_length:
                flag = True
                break

        if flag:
            pilist[index][1] = [pilist[index][1][a] + rectify[a] for a in range(3)]
            # 更新 index 到 find_id 间的 管道
            for o in range(index, find_id-1):
                pilist[o][0] = [pilist[o][0][i] + rectify[i] for i in range(3)]
                pilist[o][1] = [pilist[o][1][i] + rectify[i] for i in range(3)]
        else:
            pilist[find_id - 1] = new_last_pi
    # pilist[j] = new_pi


def Pi_bypass(original_pilist, block_wall_list, BlockBase, block_wall_list_recursion, PipeGroup_Type, ceilling):
    adjuster = MutiPipeAdjuster()
    change_flag = False
    unchanged_len_pilist = copy.deepcopy(original_pilist)
    pilist = copy.deepcopy(original_pilist)
    if block_wall_list == []:
        print("该管道附近无障碍物")
        return change_flag, original_pilist, [[], []]

    wall_list = []
    block_list = []
    for i in range(len(block_wall_list_recursion)):
        type = check([block_wall_list_recursion[i]], BlockBase)
        if type == 'Wall':
            wall_list.append(block_wall_list_recursion[i])
        else:
            block_list.append(block_wall_list_recursion[i])

    wall_list_selected = blocks_select(unchanged_len_pilist, wall_list, 3000)

    cross_position_list = []
    fail_position_list = []
    for j in range(len(unchanged_len_pilist)):  # 当前列的每一段管道与障碍物避障
        if j != 0:  # 若当前管道不是第一段管道，将上一段管道传入避障函数中
            last_pi = unchanged_len_pilist[j - 1]
        else:
            last_pi = None

        if j == len(unchanged_len_pilist) - 1:
            endlist_flag = True
        else:
            endlist_flag = False

        for wall in wall_list_selected:  # 判断管道和墙是否发生碰撞
            is_collision = collision_detection(unchanged_len_pilist[j], wall)
            if is_collision:
                cos_value = get_cos_value(unchanged_len_pilist[j], wall)
                print("管道和墙发生碰撞", cos_value)

                if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                    information = Avoid_wall(last_pi, unchanged_len_pilist[j], wall, block_list,
                                             wall_list_selected, ceilling)
                    fail_position_list.append(information[3])
                    if information[0] != unchanged_len_pilist[j]:
                        change_flag = True
                        pilist_update(pilist, unchanged_len_pilist, j, information)  # 将后面的管道衔接上
                    break
                else:
                    print("管道和墙体平行碰撞")

        Currentpipe_fail_avoid = False  # 当前管道段避障失败标志
        for i in range(len(block_list)):
            block_list_leave_changed = copy.deepcopy(block_list)
            is_collision = collision_detection(unchanged_len_pilist[j], block_list[i])
            type = check([block_list[i]], BlockBase)
            if is_collision:
                block_list_leave_changed[i] = None  # 将当前障碍物从列表中删除，用于传入避障函数
                block_list_leave_changed = list(filter(None, block_list_leave_changed))

                new_pi, rectify, new_last_pi, position, recursion_position = Avoid_obstacles(last_pi,
                                                                                             unchanged_len_pilist[j],
                                                                                             block_list[i],
                                                                                             block_list_leave_changed,
                                                                                             type, BlockBase,
                                                                                             wall_list_selected,
                                                                                             PipeGroup_Type, ceilling,
                                                                                             endlist_flag)
                new_pi = adjuster.adjust_MutiPipe_FittingPoint(new_pi)
                cross_position = position[0]
                fail_position = position[1]
                cross_position_list.append(cross_position)
                fail_position_list.append(fail_position)
                if recursion_position is not None:
                    for cross_point in recursion_position[0]:
                        cross_position_list.append(cross_point)
                    for fail_point in recursion_position[1]:
                        fail_position_list.append(fail_point)

                if new_pi != [unchanged_len_pilist[j]]:  # 若生成新的管道，将后面的管道衔接上
                    change_flag = True
                    copy_unchanged_len_pilist = copy.deepcopy(unchanged_len_pilist[j])
                    for o in range(j + 1, len(unchanged_len_pilist)):
                        unchanged_len_pilist[o][0][0] = unchanged_len_pilist[o][0][0] + rectify[0]
                        unchanged_len_pilist[o][0][1] = unchanged_len_pilist[o][0][1] + rectify[1]
                        unchanged_len_pilist[o][0][2] = unchanged_len_pilist[o][0][2] + rectify[2]
                        unchanged_len_pilist[o][1][0] = unchanged_len_pilist[o][1][0] + rectify[0]
                        unchanged_len_pilist[o][1][1] = unchanged_len_pilist[o][1][1] + rectify[1]
                        unchanged_len_pilist[o][1][2] = unchanged_len_pilist[o][1][2] + rectify[2]
                    if new_last_pi is not None and j != 0:  # 若发生平移避障，则需更新前一段管道
                        unchanged_len_pilist[j - 1] = new_last_pi
                    if len(new_pi) == 1:
                        unchanged_len_pilist[j] = new_pi[0]

                    if cross_position is not None:
                        Currentpipe_fail_avoid = False
                    # if new_pi[0][5] !=unchanged_len_pilist[j][5]:
                    #     Currentpipe_fail_avoid = True
                else:
                    Currentpipe_fail_avoid = True
            else:
                continue

            if Currentpipe_fail_avoid:
                pilist[j][5] = new_pi[0][5]
                break
            else:  # 将避障后的管道添加到原管道中
                find_id = [i for i, subarray in enumerate(pilist) if subarray[0:2] == copy_unchanged_len_pilist[0:2]]
                find_id = find_id[0]
                pilist.pop(find_id)
                for z in range(0, len(new_pi)):
                    pilist.insert(find_id + z, new_pi[z])
                for o in range(find_id + len(new_pi), len(pilist)):
                    pilist[o][0][0] = pilist[o][0][0] + rectify[0]
                    pilist[o][0][1] = pilist[o][0][1] + rectify[1]
                    pilist[o][0][2] = pilist[o][0][2] + rectify[2]
                    pilist[o][1][0] = pilist[o][1][0] + rectify[0]
                    pilist[o][1][1] = pilist[o][1][1] + rectify[1]
                    pilist[o][1][2] = pilist[o][1][2] + rectify[2]
                if new_last_pi is not None and find_id != 0:  # 若发生平移避障，则需更新前一段管道
                    pilist[find_id - 1] = new_last_pi
                break

    cross_position_list = list(filter(None, cross_position_list))
    fail_position_list = list(filter(None, fail_position_list))
    position_list = [cross_position_list, fail_position_list]
    return change_flag, pilist, position_list


def Set_slope(pilist, PipeGroup_Type, ceilling):
    change_flag = False
    if PipeGroup_Type == 'LinePipe':  # 若管道为点线水管
        for i in range(0, len(pilist)):
            change_flag = True
            len_pi = math.sqrt(
                (pilist[i][1][0] - pilist[i][0][0]) ** 2 + (pilist[i][1][1] - pilist[i][0][1]) ** 2)  # xy平面上的距离
            step = len_pi / 100  # z坐标应该下降的高度
            pilist[i][1][2] = pilist[i][1][2] - step
            if i != len(pilist) - 1:  # 将后面的管道衔接上
                for j in range(i + 1, len(pilist)):
                    pilist[j][0][2] = pilist[j][0][2] - step
                    pilist[j][1][2] = pilist[j][1][2] - step
    elif PipeGroup_Type == 'Pipe':  # 若管道为实体水管
        for i in range(0, len(pilist)):
            if (pilist[i][0][2] == pilist[i][1][2]):
                change_flag = True
                len_pi = math.sqrt(
                    (pilist[i][1][0] - pilist[i][0][0]) ** 2 + (pilist[i][1][1] - pilist[i][0][1]) ** 2)  # xy平面上的距离
                step = len_pi / 100  # z坐标应该下降的高度
                pilist[i][1][2] = pilist[i][1][2] - step
                if i != len(pilist) - 1:  # 将后面的管道衔接上
                    for j in range(i + 1, len(pilist)):
                        pilist[j][0][2] = pilist[j][0][2] - step
                        pilist[j][1][2] = pilist[j][1][2] - step
    return change_flag, pilist

from scipy.optimize import minimize
class MutiPipeAdjuster:
    def __init__(self):
        pass

    @staticmethod
    def calculate_angle(v1, v2):
        dot_product = np.dot(v1, v2)
        norm_v1 = np.linalg.norm(v1)
        norm_v2 = np.linalg.norm(v2)
        cosine_angle = dot_product / (norm_v1 * norm_v2)
        angle_radians = np.arccos(np.clip(cosine_angle, -1.0, 1.0))
        angle_degrees = np.degrees(angle_radians)
        return angle_degrees

    @staticmethod
    def adjust_midpoint(start_point, mid_point, end_point):
        vector1 = np.array(mid_point) - np.array(start_point)
        vector2 = np.array(mid_point) - np.array(end_point)
        angle = MutiPipeAdjuster.calculate_angle(vector1, vector2)
        print(round(angle, 1))

        if round(angle, 1) < 89.9:
            # 计算内积

            # 调整mid_point
            result = minimize(objective_function, mid_point, args=(start_point, end_point), method='L-BFGS-B')

            # 获取调整后的中点
            adjusted_mid_point = result.x
            print(list(adjusted_mid_point))
            print(MutiPipeAdjuster.calculate_angle(np.array(adjusted_mid_point) - np.array(start_point),
                                          np.array(adjusted_mid_point) - np.array(end_point)))
            return list(adjusted_mid_point)
        else:
            return mid_point

    def adjust_MutiPipe_FittingPoint(self, new_pi):
        if len(new_pi) > 1:
            print(len(new_pi))
            for i in range(len(new_pi) - 1):
                mid_point = new_pi[i][1]
                start_point = new_pi[i][0]
                end_point = new_pi[i + 1][1]
                mid_adjust_point = self.adjust_midpoint(start_point, mid_point, end_point)
                if mid_point != mid_adjust_point:
                    new_pi[i][1] = new_pi[i + 1][0] = mid_adjust_point
        return new_pi

def angle_between_vectors(vector1, vector2):
    # 计算向量夹角
    dot_product = np.dot(vector1, vector2)
    norm_product = np.linalg.norm(vector1) * np.linalg.norm(vector2)
    cos_theta = dot_product / norm_product
    angle_rad = np.arccos(np.clip(cos_theta, -1.0, 1.0))
    return np.degrees(angle_rad)

def objective_function(mid_point, start_point, end_point):
    # 定义优化目标函数，即夹角与90度的差值的平方
    vector1 = np.array(mid_point) - np.array(start_point)
    vector2 = np.array(mid_point) - np.array(end_point)
    angle_diff = np.abs(angle_between_vectors(vector1, vector2) - 90.5)
    return angle_diff ** 2

# 重构管道的起点
class DataAdjuster:
    def __init__(self, data):
        # 初始化数据
        self.data = data

    def compare_coordinates(self, coord1, coord2):
        # 比较坐标，如果Z轴不同，返回Z轴较大的坐标；如果Z轴相同，返回距离原点更近的坐标
        if coord1[2] != coord2[2]:
            return coord1 if coord1[2] >= coord2[2] else coord2
        else:
            distance1 = (coord1[0] ** 2 + coord1[1] ** 2) ** 0.5
            distance2 = (coord2[0] ** 2 + coord2[1] ** 2) ** 0.5
            return coord1 if distance1 <= distance2 else coord2

    def reverse_sublists(self):
        # 反转子列表
        reversed_data = []
        for pipe in reversed(self.data):
            pipe[0], pipe[1] = pipe[1], pipe[0]
            reversed_data.append(pipe)
        self.data = reversed_data

    def adjust_data(self):
        if len(self.data) == 1:
            # 如果数据长度为1，直接比较坐标并调整
            start_point = self.compare_coordinates(self.data[0][0], self.data[0][1])
            end_point = self.data[0][1] if start_point == self.data[0][0] else self.data[0][0]
            self.data[0][0], self.data[0][1] = start_point, end_point
        elif len(self.data) > 1:
            # 如果数据长度大于1，比较第一个和最后一个子列表的坐标，并根据需要反转子列表
            start_point_first = self.compare_coordinates(self.data[0][0], self.data[-1][1])
            if start_point_first == self.data[-1][1]:
                self.reverse_sublists()

        return self.data

    def reveresd_adjust_data(self, pilist, copy_pilist):
        if len(self.data) == 1:
            # 如果数据长度为1，直接比较坐标并调整
            if pilist[0] != copy_pilist[0]:
                # print("不一样")
                start_point = self.data[0][1]
                end_point = self.data[0][0]
                self.data[0][0], self.data[0][1] = start_point, end_point
            # else:
            #     print("一样")
        elif len(self.data) > 1:

            if pilist[0][0] == copy_pilist[-1][1]:
                self.reverse_sublists()
                print("多段，不一样")
            else:
                print("多段，一样")

        return self.data